#include <RCSwitch.h>

//#define __D__

#define MOTO_A      6
#define MOTO_B      9
#define LED         13
#define DECT_PIN    A5
//#define SPEED       128

#define DECT_TOUT   100                 /* 电压检测周期 */
#define DECT_CNT    10                  /* 电压检测次数 */
#define TIMEOUT     240

#define FORWARD     0x5D30
#define STOP        0x5D0C
#define LEFT        0x5D03
#define RIGHT       0x5DC0

#define MOTION_TLD_MIN  5.8             /* 禁止动力工作的最低电压阈值 */
#define MOTION_TLD_MAX  6.0             /* 恢复动力工作的最低电压阈值 */

RCSwitch receiver = RCSwitch ();
long last_received = -1;                /* 记录上次接收到指令的时间 */
long last_command  = -1;                /* 记录上次接收到的指令值 */
long turning       = -1;                /* 记录当前转弯的方向 */
long last_dect     = -1;                /* 记录上次检查电池电压的时间 */

int speeds[4] = { 64, 128, 192, 255 };  /* 4 档速率对应的值 */
int index     = 0;                      /* 记录选择当前速率的档位 */
int SPEED     = speeds[index];          /* 当前速率 */

int volt_idx  = 0;
int volt_rect[DECT_CNT];
uint8_t motion_enabled;

void setup() {
#ifdef __D__
    Serial.begin (9600);
#endif
    receiver.enableReceive (0);

    pinMode (LED, OUTPUT);
    pinMode (MOTO_A, OUTPUT);
    pinMode (MOTO_B, OUTPUT);
    pinMode (DECT_PIN, INPUT);
    digitalWrite (LED, LOW);
    digitalWrite (MOTO_A, LOW);
    digitalWrite (MOTO_B, LOW);
    digitalWrite (DECT_PIN, LOW);
    /* 禁止动作 */
    motion_enabled = 0;
#ifdef __D__
    Serial.println ("setup done.");
#endif
}

void loop() {
    long now = millis ();

    volt_detect ();
    
    if (receiver.available ()) {
        int value = receiver.getReceivedValue ();
#ifdef __D__
        Serial.print ("[");
        Serial.print (now);
        Serial.print ("] received: 0x");
        Serial.println (value, HEX);
#endif
        switch (value) {
            case FORWARD:
                if (last_command == FORWARD) {
                    int delta = now - last_received;
#ifdef __D__
                    Serial.print ("delta time = ");
                    Serial.print (delta);
                    Serial.println (" ms.");
#endif
                    if (delta > 360) { 
                        index = (++ index) % 4;
                        SPEED = speeds[index];
#ifdef __D__
                        Serial.print ("set speed to ");
                        Serial.println (SPEED);
#endif
                    }
                }
                forward ();
                break;
                
            case STOP:
                if (last_command != value && last_command != -1) {
                    _stop ();
                }
                break;
                
            case LEFT:
                if (turning != value) {
                    turn_left ();
                }
                break;
                
            case RIGHT:
                if (turning != value) {
                    turn_right ();
                }
                break;
        }
        receiver.resetAvailable ();
        last_received = millis ();
    }

    if (turning != -1 && millis () - last_received > TIMEOUT) {
        cancel_turning ();
    }
}

/**
 * 前进
 */
void forward () {
    if (!motion_enabled) return;
    analogWrite (MOTO_A, SPEED);
    analogWrite (MOTO_B, SPEED);
    last_command = FORWARD;
#ifdef __D__
    Serial.println ("Forward!");
#endif
}

/**
 * 停止
 */
void _stop () {
    digitalWrite (MOTO_A, LOW);
    digitalWrite (MOTO_B, LOW);
#ifdef __D__
    Serial.println ("Stop and reset speed to zero.");
#endif
    last_command = -1;
    index = 0;
    SPEED = speeds[0];
}

/**
 * 左转
 */
void turn_left () {
    if (!motion_enabled) return;
    if (turning == RIGHT) {
#ifdef __D__
        Serial.println ("perv turning is right, stop it and wait for a second");
#endif
        cancel_turning ();
    }
    digitalWrite (MOTO_A, LOW);
    analogWrite (MOTO_B, SPEED);
#ifdef __D__
    Serial.println ("Begin turn left");
#endif
    turning = LEFT;
}

/**
 * 右转
 */
void turn_right () {
    if (!motion_enabled) return;
    if (turning == LEFT) {
#ifdef __D__
        Serial.println ("prev turning is left, stop it and wait for a second");
#endif
        cancel_turning ();
    }
    analogWrite (MOTO_A, SPEED);
    digitalWrite (MOTO_B, LOW);
    Serial.println ("Begin turn right");
    turning = RIGHT;
}

/**
 * 取消转弯
 */
void cancel_turning () {
    if (last_command == FORWARD) {
        forward ();
    } else if (last_command == STOP || last_command == -1) {
        _stop ();
    }
#ifdef __D__
    Serial.println ("cancel turning");
#endif
    turning = -1;
}

void volt_detect () {
    long now = millis ();
    if (now - last_dect >= DECT_TOUT) {
        int n = analogRead (DECT_PIN);
        n = map (n, 0, 1023, 0, 498);
        volt_rect[volt_idx ++] = n;
        if (volt_idx >= DECT_CNT) {
            long x = 0;
            for (int i = 0;i < DECT_CNT; i ++) {
                x += volt_rect[i];
                volt_rect[i] = 0;
            }

            double volt = 2 * x / 100.0 / DECT_CNT;
            if (volt <= MOTION_TLD_MIN) motion_enabled = 1;
            else if (volt >= MOTION_TLD_MAX) {
                motion_enabled = 0;
                _stop ();
            }
#ifdef __D__
            Serial.print ("volt = ");
            Serial.println (volt);
#endif
            volt_idx = 0;
        }
        last_dect = now;
    }
}
